 %% scripts to read data_set and solve with different solvers
% To keep consistent with the simulator, the iter and err information will
% be saved in the obj.solution.err and obj.solution.iter
function[] = ECCOMAS_hdf5()
%data_source = 'simulator';
data_source = 'ode';
if strcmp(data_source, 'simulator')
    load('data_set_Lemke.mat');
else
    % data_set = h5load('chain.h5');
    disp('Loading hdf5 file...'); 
    data_set = h5load('logs.h5');
    disp('Done.');
end
 
START_STEP = 79;
END_STEP = 88;
MAX_STEP  = END_STEP - START_STEP + 1;
solution_info = struct();
solution_info.itemize_error = zeros(MAX_STEP, 4);
solution_info.contact_counting = zeros(MAX_STEP, 3);
solution_info.iter = 0;
 
for STEP = START_STEP : END_STEP
    solution_info.iter = solution_info.iter + 1;
    disp(STEP);
    CurrentFrame = sprintf('frame%06d', STEP);
    % data_set.(frame_num) has the property of bodies, contacts,
    % constraints, dynamics and solution.     
   
    %% mNCP formulation
    % compute the predynamics 
    data_set.(CurrentFrame).dynamics = struct();
   
    
    data_set.(CurrentFrame).dynamics = preDynamics(data_set.(CurrentFrame), 'mNCP');
     
    % call the solver and get the solution 
%     solverName = 'fixed_point_pgs_w';
%     data_set.(CurrentFrame).solution.FixedPGSNCPSoln = struct();
%     data_set.(CurrentFrame).solution.FixedPGSNCPSoln = SubProblem(data_set.(CurrentFrame), 'mNCP', solverName);
    
    solverName = 'prox_NCP';
  
    data_set.(CurrentFrame).solution_info = solution_info;
    solution_info = simStepRun(data_set.(CurrentFrame), 'mNCP', solverName); 
    %% pure LCP formulation 
    % compute the predynamics 
%     data_set.(CurrentFrame).dynamics = struct();
%     data_set.(CurrentFrame).dynamics = PreDynamicsWFormulation(data_set.(CurrentFrame), 'LCP');
%  
%     
%     solverName = 'PGS';
%     data_set.(CurrentFrame).solution.PGSSoln = struct();   
%     data_set.(CurrentFrame).solution.PGSSoln = SubProblem(data_set.(CurrentFrame), 'LCP', solverName);  
end
 
disp('DONE');
    
% Plot the error and the bar plot 
solution_error = solution_info.itemize_error; % size: MAX_STEP X 5 [norml_error, slide_align, stick_residual, slide_cone_satisfy]
contactError= solution_info.contact_counting; % size: MAX_STEP X 3 [slide_badAlign, stick, slide]

FigHandle1 = figure;
set(FigHandle1, 'Position', [100, 100, 700, 500]);

subplot(2,1,1);
%plot(xaxis,solution_error, 'LineWidth', 2);
solution_error = solution_error(:, 1:3);
solution_error = solution_error + rand(size(solution_error)) ./ 1000;
semilogy((START_STEP:1:END_STEP)', solution_error, 'LineWidth', 2);
legend('Nonpenetration error', 'Slide alignment error ', 'Stick residual velocity');

%legend('Nonpenetration error', 'Slide alignment error ', 'Stick residual velocity', 'slide cone satisfication');
%xlabel('Time step', 'FontSize', 12, 'FontWeight', 'bold');
ylim([10^(-6), 10^2]);
ylabel('Measured Error', 'FontSize', 12, 'FontWeight', 'bold');
title('Itemized error versus time step - NCP-PGS(prox)', 'FontSize', 12, 'FontWeight', 'bold');

subplot(2,1,2);
H = bar((START_STEP:1:END_STEP)', contactError,'stacked');
mycolor = {'r', 'b', 'g'};
for k=1:3
  set(H(k),'Facecolor',mycolor{k});
end
xlim([START_STEP-1, END_STEP+1]); 
ylim([0, 300]);
legend('Slide: incorrect direction', 'Stick', 'Slide');
xlabel('Time step', 'FontSize', 12, 'FontWeight', 'bold');
ylabel('Num of Contacts', 'FontSize', 12, 'FontWeight', 'bold');
%title('Number of contact at different states', 'FontSize', 12, 'FontWeight', 'bold');

end